#!/usr/bin/python

# Copyright (c) <2016>, <Nanyang Technological University> All rights reserved.

# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:

# 1. Redistributions of source code must retain the above copyright notice,
# this list of conditions and the following disclaimer.

# 2. Redistributions in binary form must reproduce the above copyright notice,
# this list of conditions and the following disclaimer in the documentation
# and/or other materials provided with the distribution.

# 3. Neither the name of the copyright holder nor the names of its contributors
# may be used to endorse or promote products derived from this software without
# specific prior written permission.

# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.

import sys
from matplotlib.lines import lineStyles
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
import scipy
from numpy import *
from statsmodels.distributions.empirical_distribution import ECDF
from tf import transformations 

if __name__ == "__main__":

    if len(sys.argv) == 2 :
        filename = sys.argv[1]
    else:
        print "Usage: python draw_results.py [filename]"
        exit(0)

    file = open(filename, 'r')
    while file.readline()[0] == '#':
        continue
    results = array([map(float, line.split(' ')) for line in file if line.strip() !=""])
    pos_est = results[:,1:4]
    rot_est = results[:,4:8]
    time = results[:,0] - results[0,0]

    fig = plt.figure()

    ax = fig.add_subplot(211)
    ax.plot(time, pos_est, marker=".")
    
    plt.legend(['est x', 'est y', 'est z'])
    plt.grid()

    ax = fig.add_subplot(212)
    plt.plot(time,rot_est,marker=".")
    plt.legend(['est qx', 'est qy', 'est qz', 'est qw'])
    plt.grid()

    fig = plt.figure()
    ax = fig.add_subplot(111, projection='3d')
    ax.plot(pos_est[:,0], pos_est[:,1], pos_est[:,2])
    
    plt.show()
